#include <ros/ros.h>
#include"std_msgs/Float64MultiArray.h"
#include"std_msgs/UInt32.h"

#include "motor.h"
#include "my_comm.h"
#include "robot_ctrl.h"
#include "dht11.h"

union _point_t remote_msg;

void remote_callback(std_msgs::Float64MultiArray msg)
{
    remote_msg.x = msg.data[0];
    remote_msg.y = msg.data[1];
    remote_msg.z = msg.data[2];
}

#define RATE_DO_EXECUTE(hz, tick)             (tick % (1000/hz) == 0)

int main(int argc, char *args[])
{
    ros::init(argc, args, "legged_ctrl");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("robot_remote", 1, remote_callback);
    ros::Publisher dht11_pub = nh.advertise<std_msgs::UInt32>("dht11_data", 1);

    std_msgs::UInt32 data_msg;

    ROS_INFO_STREAM("legged ctrl start succeed!");

    ros::Rate rate(1000);
    
    comm_init();
    GPIO_init(7);

    uint32_t tick=0;
    while (ros::ok())
    {
        rate.sleep();
        
        if (RATE_DO_EXECUTE(10, tick)) {        /* 10hz处理控制指令 */
            ros::spinOnce();
        }

        if (RATE_DO_EXECUTE(2, tick)) {        /* 10hz处理控制指令 */
            
            data_msg.data = dht11_update();
            if (data_msg.data) {
                dht11_pub.publish(data_msg);
            }
        }

        if (RATE_DO_EXECUTE(50, tick)) {        /* 机器人控制 */
			robot_ctrl();
		}

        if (RATE_DO_EXECUTE(50, tick)) {        /* 发送电机位罿 */
			motor_ctrl_task();
		}

        tick++;
        
    }

    comm_exit();
    return 0;
}